#ifndef _PID2_
#define _PID2_
#include <SetMotor_Code.h>
#include <GetData.h>
#include <GetMotorCode.h>


float pid_Kp = 10, pid_Ki = 0.05, pid_Kd = 0;         
float pid_error = 0, pid_P = 0, pid_I = 0, pid_D = 0, PID2_value = 0;

float previous_error2 = 0, previous_I2 = 0;         
 int left_motor_speed2, right_motor_speed2;
int initial_motor_speed2 = 0;           
static int ispidinit2=0;
void read_sensor_values2(void);                    
void calc_pid2(void);                              
void motor_control2(void);                        
 

void motorsWrite3(int speedL, int speedR)
{
  if(speedL>=0)
  {
    SetMotor_Code(1, 0,speedL / 2.55);
  }
  else
  {
    SetMotor_Code(1, 2,-speedL / 2.55);
  }
  
  if(speedR>=0)
  {
    SetMotor_Code(2, 0,speedR / 2.55);
  }
  else
  {
    SetMotor_Code(2, 2,-speedR / 2.55);
  }
}

void read_sensor_values2()
{
  pid_error=abs(GetMotorCode(1))-abs(GetMotorCode(2));
   
}
void calc_pid2()
{

  pid_P = pid_error;
  pid_I = pid_I + pid_error;
  pid_D = pid_error - previous_error2;
 
  PID2_value = (pid_Kp * pid_P) + (pid_Ki * pid_I) + (pid_Kd * pid_D);
  
  previous_error2 = pid_error;
}
void motor_control2()
{
  if(initial_motor_speed2>=0)
  {
    left_motor_speed2 = initial_motor_speed2*2.55 - PID2_value;
    right_motor_speed2 = initial_motor_speed2*2.55 + PID2_value;
  }
  else
  {
    left_motor_speed2 = -initial_motor_speed2*2.55 - PID2_value;
    right_motor_speed2 = -initial_motor_speed2*2.55 + PID2_value;
  }
  
  
  if(left_motor_speed2 < -255){
    left_motor_speed2 = -255;
  }
  if(left_motor_speed2 > 255){
    left_motor_speed2 = 255;
  }

  if(right_motor_speed2 < -255){
    right_motor_speed2 = -255;
  }
  if(right_motor_speed2 > 255){
    right_motor_speed2 = 255;
  }
if(initial_motor_speed2>=0)
  motorsWrite3(left_motor_speed2,-right_motor_speed2);
else
  motorsWrite3(-left_motor_speed2,right_motor_speed2);
  
 //SetLCD5Char(0, 0, left_motor_speed2, YELLOW, BLACK);
 //SetLCD5Char(0,32, right_motor_speed2, YELLOW, BLACK);

//SetLCD5Char(110, 0, GetMotorCode(1), YELLOW, BLACK);
 //SetLCD5Char(110,32, GetMotorCode(2), YELLOW, BLACK);
 
} 


#endif